%UWB的采样评率为25HZ
dis_select_UWB= select(bag, 'Time',[bag.StartTime bag.EndTime], 'Topic', '/uav1/nlink_linktrack_nodeframe3');
disMsgs_UWB= readMessages(dis_select_UWB,'DataFormat','struct');

dis_old =zeros(4144,3);
for i=1:4144
    sizeofdis=size(disMsgs_UWB{i, 1}.Nodes);
    if sizeofdis(2)==3
        dis_old(i,1)=disMsgs_UWB{i}.Nodes(1).Dis;
        dis_old(i,2)=disMsgs_UWB{i}.Nodes(2).Dis;
        dis_old(i,3)=disMsgs_UWB{i}.Nodes(3).Dis;
    elseif sizeofdis(2)==2
        
        if disMsgs_UWB{i, 1}.Nodes(1).Id==0&& disMsgs_UWB{i, 1}.Nodes(2).Id==1
            dis_old(i,1)=disMsgs_UWB{i}.Nodes(1).Dis;
            dis_old(i,2)=disMsgs_UWB{i}.Nodes(2).Dis;
            dis_old(i,3)=dis_last(3);
        elseif disMsgs_UWB{i, 1}.Nodes(1).Id==0&& disMsgs_UWB{i, 1}.Nodes(2).Id==3
            dis_old(i,1)=disMsgs_UWB{i}.Nodes(1).Dis;
            dis_old(i,3)=disMsgs_UWB{i}.Nodes(2).Dis;
            dis_old(i,2)=dis_last(2);
        else
            dis_old(i,2)=disMsgs_UWB{i}.Nodes(1).Dis;
            dis_old(i,3)=disMsgs_UWB{i}.Nodes(2).Dis;
            dis_old(i,1)=dis_last(1);
        end
    else 
        if disMsgs_UWB{i, 1}.Nodes(1).Id==0
            dis_old(i,1)=disMsgs_UWB{i}.Nodes(1).Dis;
            dis_old(i,2)=dis_last(2);
            dis_old(i,3)=dis_last(3);
        elseif disMsgs_UWB{i, 1}.Nodes(1).Id==1
            dis_old(i,2)=disMsgs_UWB{i}.Nodes(1).Dis;
            dis_old(i,1)=dis_last(1);
            dis_old(i,3)=dis_last(3);
        else 
            dis_old(i,3)=disMsgs_UWB{i}.Nodes(1).Dis;
            dis_old(i,2)=dis_last(2);
            dis_old(i,1)=dis_last(1);
        end
    end
    dis_last=dis_old(i,1:3); 
end

%这里选择用ros的时间而不是UWB自带的时间
t_relative_UWB=table2array(dis_select_UWB.MessageList(:,1))-table2array(dis_select_UWB.MessageList(1,1));

%顺时针的顺序为0132   实验时发现ID2无效
x_off=0;
y_off=0;
z_off=0;
x0=x_off-0.66;
y0=y_off+0.52;
z0=z_off;
x1=x_off+0.4;
y1=y_off+0.52;
z1=z_off+0.5;
x2=x_off-0.66;
y2=y_off-0.54;
z2=z_off+0.5;
x3=x_off+0.4;
y3=y_off-0.54;
z3=z_off;

%%
%sg滤波
dis_sg=sgolayfilt(dis_old,3,11);
%%
%解算位置
%从视觉反算出的距离，用于验证结果的正确性
x_test=pose_vision_x-x0;
y_test=pose_vision_y-y0;
z_test=pose_vision_z-z0;
dis_test_0=sqrt(x_test.^2+y_test.^2+z_test.^2);
x_test=pose_vision_x-x1;
y_test=pose_vision_y-y1;
z_test=pose_vision_z-z1;
dis_test_1=sqrt(x_test.^2+y_test.^2+z_test.^2);
x_test=pose_vision_x-x3;
y_test=pose_vision_y-y3;
z_test=pose_vision_z-z3;
dis_test_3=sqrt(x_test.^2+y_test.^2+z_test.^2);
%最小二乘法
dis_test(:,1)=dis_test_0;
dis_test(:,2)=dis_test_1;
dis_test(:,3)=dis_test_3;
% point_leastsqr=least_square_new(dis_test,4144);
% point_leastsqr=least_square_new(dis_sg,4144);
% %几何法
point_threeball=zeros(4144,3);
for i=1:4144
    point_threeball(i,:)=threeball(dis_sg(i,1),dis_sg(i,2),dis_sg(i,3));
%     point_threeball(i,:)=threeball(dis_test(i,1),dis_test(i,2),dis_test(i,3));
end